<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvros/qvssba.cpp</h1><a href="qvssba_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00024"></a>00024 
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;qvros.h&gt;</span>
<a name="l00026"></a>00026 
<a name="l00027"></a>00027 <span class="preprocessor">#include &lt;ros/ros.h&gt;</span>
<a name="l00028"></a>00028 <span class="preprocessor">#include &lt;sba/sba.h&gt;</span>
<a name="l00029"></a>00029 <span class="preprocessor">#include &lt;sba/sba_file_io.h&gt;</span>
<a name="l00030"></a>00030 <span class="preprocessor">#include &lt;sba/visualization.h&gt;</span>
<a name="l00031"></a>00031 <span class="preprocessor">#include &lt;visualization_msgs/Marker.h&gt;</span>
<a name="l00032"></a>00032 <span class="preprocessor">#include &lt;time.h&gt;</span>
<a name="l00033"></a>00033 <span class="preprocessor">#include &lt;map&gt;</span>
<a name="l00034"></a>00034 <span class="preprocessor">#include &lt;qvsfm.h&gt;</span>
<a name="l00035"></a>00035 
<a name="l00036"></a>00036 <span class="keyword">using namespace </span>sba;
<a name="l00037"></a>00037 <span class="keyword">using namespace </span>std;
<a name="l00038"></a>00038 <span class="keyword">using namespace </span>Eigen;
<a name="l00039"></a>00039 <span class="keyword">using namespace </span>frame_common;
<a name="l00040"></a>00040 
<a name="l00041"></a>00041 <span class="preprocessor">#include &lt;QTime&gt;</span>
<a name="l00042"></a>00042 
<a name="l00043"></a>00043 <span class="comment">//template &lt;typename SysSBAType&gt;</span>
<a name="l00044"></a>00044 <span class="keywordtype">int</span> initReconstructionToSysSBA( SysSBA&amp; sbaout,
<a name="l00045"></a>00045                                 <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00046"></a>00046                                 <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00047"></a>00047                                 <span class="keyword">const</span> QList&lt; QHash&lt; int, QPointF&gt; &gt; &amp;pointsProjections)
<a name="l00048"></a>00048         {
<a name="l00049"></a>00049         <span class="keyword">const</span> <span class="keywordtype">int</span> ncams = cameraPoses.count();
<a name="l00050"></a>00050         <span class="keyword">const</span> <span class="keywordtype">int</span> npts  = points3D.count();
<a name="l00051"></a>00051 
<a name="l00052"></a>00052         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;ncams; i++)
<a name="l00053"></a>00053                 {
<a name="l00054"></a>00054                 <span class="comment">// camera params</span>
<a name="l00055"></a>00055                 <span class="comment">//Vector3d &amp;camp = camps[i];</span>
<a name="l00056"></a>00056                 CamParams cpars = { -1.0, -1.0, 0, 0, 0 }; <span class="comment">// set focal length, no offset</span>
<a name="l00057"></a>00057 
<a name="l00058"></a>00058                 <span class="comment">// Rotation.</span>
<a name="l00059"></a>00059                 <span class="keyword">const</span> <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> &amp;cameraPose = cameraPoses[i];
<a name="l00060"></a>00060                 <span class="keyword">const</span> <a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a> q = (<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(1.0, 0.0, 0.0, 0.0) * cameraPose.<a class="code" href="classQVCameraPose.html#a8643ae492e7a6f66679bf0cc2fc911c7" title="Orientation of the camera pose.">getOrientation</a>()).inverse();
<a name="l00061"></a>00061                 Quaternion&lt;double&gt; frq; <span class="comment">// camera frame rotation, from Bundler docs</span>
<a name="l00062"></a>00062                 frq.x() = q.<a class="code" href="classQVQuaternion.html#aebb9e948cd7fa6e1b60e4a27f573167d" title="Returns the coordinate i of the cuaternion.">i</a>();
<a name="l00063"></a>00063                 frq.y() = q.<a class="code" href="classQVQuaternion.html#adad1a0e10ae3d3413fe96b59ef103d26" title="Returns the coordinate j of the cuaternion.">j</a>();
<a name="l00064"></a>00064                 frq.z() = q.<a class="code" href="classQVQuaternion.html#ae7f769c05a5ba57ec0d776d88741eeae" title="Returns the coordinate k of the cuaternion.">k</a>();
<a name="l00065"></a>00065                 frq.w() = q.<a class="code" href="classQVQuaternion.html#a4b71d76116709c6a6d1872bd1f137c9e" title="Returns the coordinate for the real component of the cuaternion.">real</a>();
<a name="l00066"></a>00066 
<a name="l00067"></a>00067                 <span class="comment">// Translation.         </span>
<a name="l00068"></a>00068                 <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> c = cameraPose.<a class="code" href="classQVCameraPose.html#a57101f0fd271736866f970c1635dd7c1" title="Center of the camera pose.">getCenter</a>();
<a name="l00069"></a>00069 
<a name="l00070"></a>00070                 Vector4d frt;
<a name="l00071"></a>00071                 frt[0] = c.<a class="code" href="classQV3DPointF.html#aab1c8e4380b126607771339778a83a1e" title="Gets the value for the first coordinate of the 3D point.">x</a>();
<a name="l00072"></a>00072                 frt[1] = c.<a class="code" href="classQV3DPointF.html#ab3999406794a8d4afa0718af691b8920" title="Gets the value for the second coordinate of the 3D point.">y</a>();
<a name="l00073"></a>00073                 frt[2] = c.<a class="code" href="classQV3DPointF.html#aba286a7e040358ecbffce7c4566cdd8d" title="Gets the value for the third coordinate of the 3D point.">z</a>();
<a name="l00074"></a>00074                 frt[3] = 1.0;
<a name="l00075"></a>00075 
<a name="l00076"></a>00076                 sbaout.addNode(frt, frq, cpars);
<a name="l00077"></a>00077                 }
<a name="l00078"></a>00078         
<a name="l00079"></a>00079         <span class="comment">// set up points</span>
<a name="l00080"></a>00080         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;npts; i++)
<a name="l00081"></a>00081                 {
<a name="l00082"></a>00082                 <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> p = points3D[i];
<a name="l00083"></a>00083                 Point pt;
<a name="l00084"></a>00084                 pt[0] = p.<a class="code" href="classQV3DPointF.html#aab1c8e4380b126607771339778a83a1e" title="Gets the value for the first coordinate of the 3D point.">x</a>();
<a name="l00085"></a>00085                 pt[1] = p.<a class="code" href="classQV3DPointF.html#ab3999406794a8d4afa0718af691b8920" title="Gets the value for the second coordinate of the 3D point.">y</a>();
<a name="l00086"></a>00086                 pt[2] = p.<a class="code" href="classQV3DPointF.html#aba286a7e040358ecbffce7c4566cdd8d" title="Gets the value for the third coordinate of the 3D point.">z</a>();
<a name="l00087"></a>00087                 pt[3] = 1;
<a name="l00088"></a>00088 
<a name="l00089"></a>00089                 sbaout.addPoint(pt);
<a name="l00090"></a>00090                 }
<a name="l00091"></a>00091                 
<a name="l00092"></a>00092         sbaout.useLocalAngles = <span class="keyword">true</span>;    <span class="comment">// use local angles</span>
<a name="l00093"></a>00093         sbaout.nFixed = 1;
<a name="l00094"></a>00094 
<a name="l00095"></a>00095         <span class="comment">// set up projections</span>
<a name="l00096"></a>00096         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> numPoint=0; numPoint&lt;npts; numPoint++)
<a name="l00097"></a>00097                 {
<a name="l00098"></a>00098                 <span class="comment">// track</span>
<a name="l00099"></a>00099                 <span class="keyword">const</span> QHash&lt; int, QPointF&gt; &amp;projections = pointsProjections[numPoint];
<a name="l00100"></a>00100                 <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> numCam, projections.keys())
<a name="l00101"></a>00101                         {
<a name="l00102"></a>00102                         <span class="keyword">const</span> QPointF p = projections[numCam];
<a name="l00103"></a>00103                         Vector2d pt;
<a name="l00104"></a>00104                         pt[0] = p.<a class="code" href="classQV3DPointF.html#aab1c8e4380b126607771339778a83a1e" title="Gets the value for the first coordinate of the 3D point.">x</a>();
<a name="l00105"></a>00105                         pt[1] = -p.y();
<a name="l00106"></a>00106                         sbaout.addMonoProj(numCam,numPoint,pt);
<a name="l00107"></a>00107                         }
<a name="l00108"></a>00108                 }
<a name="l00109"></a>00109         <span class="keywordflow">return</span> 0;
<a name="l00110"></a>00110         }
<a name="l00111"></a>00111 
<a name="l00112"></a>00112 <span class="comment">//template &lt;typename SysSBAType&gt;</span>
<a name="l00113"></a>00113 <span class="keywordtype">int</span> recoverReconstructionFromSysSBA(SysSBA &amp;sbain, QList&lt;QVCameraPose&gt; &amp;cameraPoses, QList&lt;QV3DPointF&gt; &amp;points3D, QList&lt; QHash&lt; int, QPointF&gt; &gt; &amp;pointsProjections)
<a name="l00114"></a>00114         {
<a name="l00115"></a>00115         <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0;
<a name="l00116"></a>00116 
<a name="l00117"></a>00117         <span class="comment">// Set up transform matrix for camera parameters</span>
<a name="l00118"></a>00118         Matrix3d m180x;         <span class="comment">// rotate 180 deg around X axis, to convert Bundler frames to SBA frames</span>
<a name="l00119"></a>00119         m180x &lt;&lt; 1, 0, 0, 0, -1, 0, 0, 0, -1;
<a name="l00120"></a>00120 
<a name="l00121"></a>00121         <span class="comment">// Then goes information about each camera, in &lt;f&gt; &lt;k1&gt; &lt;k2&gt;\n&lt;R&gt;\n&lt;t&gt; format.</span>
<a name="l00122"></a>00122         <span class="keywordflow">for</span> (i = 0; i &lt; sbain.nodes.size(); i++)
<a name="l00123"></a>00123                 {
<a name="l00124"></a>00124                 Quaternion&lt;double&gt; quat(sbain.nodes[i].qrot);
<a name="l00125"></a>00125                 quat.normalize();
<a name="l00126"></a>00126                 Matrix3d rotmat = m180x * quat.toRotationMatrix().transpose();
<a name="l00127"></a>00127                 Matrix3d &amp;Kcam = sbain.nodes[i].Kcam;
<a name="l00128"></a>00128 
<a name="l00129"></a>00129                 <span class="comment">//Vector3d trans = sbain.nodes[i].trans.start&lt;3&gt;();</span>
<a name="l00130"></a>00130                 <span class="comment">//trans = -rotmat*trans;</span>
<a name="l00131"></a>00131 
<a name="l00132"></a>00132                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> R(3,3), K(3,3);
<a name="l00133"></a>00133                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> idx = 0; idx &lt; 3; idx++)
<a name="l00134"></a>00134                         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> jdx = 0; jdx &lt; 3; jdx++)
<a name="l00135"></a>00135                                 {
<a name="l00136"></a>00136                                 R(idx,jdx) = rotmat(idx,jdx);
<a name="l00137"></a>00137                                 K(idx,jdx) = Kcam(idx, jdx);
<a name="l00138"></a>00138                                 }
<a name="l00139"></a>00139                 K(2,2) = -K(2,2);
<a name="l00140"></a>00140 
<a name="l00141"></a>00141                 <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> transX(      sbain.nodes[i].trans(0,0),
<a name="l00142"></a>00142                                                         sbain.nodes[i].trans(1,0),
<a name="l00143"></a>00143                                                         sbain.nodes[i].trans(2,0) );
<a name="l00144"></a>00144 
<a name="l00145"></a>00145                 transX = -R * transX;
<a name="l00146"></a>00146 
<a name="l00147"></a>00147                 <span class="keyword">const</span> <span class="keywordtype">double</span> errorK = ( <a class="code" href="classQVMatrix.html#a6eb15980d17c17d4a55ab09a77bcd715" title="Creates an identity matrix.">QVMatrix::identity</a>(3) + K ).norm2();
<a name="l00148"></a>00148                 <span class="keywordflow">if</span> (errorK != 0.0)
<a name="l00149"></a>00149                         std::cout &lt;&lt; <span class="stringliteral">&quot;**** ERROR: sSBA changed the intrinsic camera parameters.&quot;</span> &lt;&lt; std::endl;
<a name="l00150"></a>00150 
<a name="l00151"></a>00151                 <span class="comment">//cameraPoses &lt;&lt; QVEuclideanMapping3(QVQuaternion(R), QV3DPointF(trans(0), trans(1), trans(2)));</span>
<a name="l00152"></a>00152                 cameraPoses &lt;&lt; <a class="code" href="classQVEuclideanMapping3.html" title="Class modeling rotation-translation transformations for 3D points.">QVEuclideanMapping3</a>(<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(R), transX);
<a name="l00153"></a>00153                 }
<a name="l00154"></a>00154 
<a name="l00155"></a>00155         <span class="comment">// Then goes information about each point. &lt;pos&gt;\n&lt;color&gt;\n&lt;viewlist&gt;</span>
<a name="l00156"></a>00156         <span class="keywordflow">for</span> (i = 0; i &lt; sbain.tracks.size(); i++)
<a name="l00157"></a>00157                 {
<a name="l00158"></a>00158                 <span class="comment">// World &lt;x y z&gt;</span>
<a name="l00159"></a>00159                 points3D &lt;&lt; <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(sbain.tracks[i].point(0), sbain.tracks[i].point(1), sbain.tracks[i].point(2));
<a name="l00160"></a>00160                 ProjMap &amp;prjs = sbain.tracks[i].projections;
<a name="l00161"></a>00161                 pointsProjections &lt;&lt; QHash&lt; int, QPointF&gt;();
<a name="l00162"></a>00162 
<a name="l00163"></a>00163                 <span class="comment">// Output all the tracks as monocular tracks.</span>
<a name="l00164"></a>00164                 <span class="keywordflow">for</span>(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
<a name="l00165"></a>00165                         {
<a name="l00166"></a>00166                         Proj &amp;prj = itr-&gt;second;
<a name="l00167"></a>00167                         <span class="comment">// y is reversed (-y)</span>
<a name="l00168"></a>00168                         Node &amp;node = sbain.nodes[prj.ndi];
<a name="l00169"></a>00169                         
<a name="l00170"></a>00170                         <span class="keywordtype">double</span> cx = node.Kcam(0, 2);
<a name="l00171"></a>00171                         <span class="keywordtype">double</span> cy = node.Kcam(1, 2);
<a name="l00172"></a>00172                         <span class="keywordtype">double</span> coorX = prj.kp(0)-cx;
<a name="l00173"></a>00173                         <span class="keywordtype">double</span> coorY = -(prj.kp(1)-cy);
<a name="l00174"></a>00174 
<a name="l00175"></a>00175                         pointsProjections[i][prj.ndi] = QPointF(coorX, coorY);
<a name="l00176"></a>00176                         }
<a name="l00177"></a>00177                 }
<a name="l00178"></a>00178         
<a name="l00179"></a>00179         <span class="keywordflow">return</span> 0;
<a name="l00180"></a>00180         } 
<a name="l00181"></a>00181 
<a name="l00182"></a><a class="code" href="group__qvros.html#gad93da3ae76d4c1cebbdd6b7dbd29d662">00182</a> <span class="keywordtype">int</span> <a class="code" href="group__qvros.html#gad93da3ae76d4c1cebbdd6b7dbd29d662" title="Apply sSBA optimization on a SfM reconstruction.">sSBAOptimization</a>(   <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00183"></a>00183                         <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00184"></a>00184                         <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointsProjections,
<a name="l00185"></a>00185                         QList&lt;QVCameraPose&gt; &amp;refinedCameraPoses,
<a name="l00186"></a>00186                         QList&lt;QV3DPointF&gt; &amp;refinedPoints3D,
<a name="l00187"></a>00187                         <span class="keywordtype">int</span> &amp;time_sSBA,
<a name="l00188"></a>00188                         <span class="keyword">const</span> <span class="keywordtype">int</span> numIters,
<a name="l00189"></a>00189                         <span class="keyword">const</span> <span class="keywordtype">double</span> lambda,
<a name="l00190"></a>00190                         <span class="keyword">const</span> <span class="keywordtype">int</span> useCSparse,
<a name="l00191"></a>00191                         <span class="keyword">const</span> <span class="keywordtype">int</span> numFixedCameras,
<a name="l00192"></a>00192                         <span class="keyword">const</span> <span class="keywordtype">double</span> initTolCG,
<a name="l00193"></a>00193                         <span class="keyword">const</span> <span class="keywordtype">int</span> maxItersCG
<a name="l00194"></a>00194                         )
<a name="l00195"></a>00195         {
<a name="l00196"></a>00196         Q_ASSERT(not <a class="code" href="group__qvsfm.html#gafea767e25db8e80b1822b8a1499776fa" title="Test correct camera cheirality.">testCheirality</a>(cameraPoses, pointsProjections));
<a name="l00197"></a>00197 
<a name="l00198"></a>00198         <span class="comment">// Init sSBA algorithm.</span>
<a name="l00199"></a>00199         SysSBA sys;
<a name="l00200"></a>00200         initReconstructionToSysSBA(sys, cameraPoses, points3D, pointsProjections);
<a name="l00201"></a>00201 
<a name="l00202"></a>00202         sys.nFixed = numFixedCameras;
<a name="l00203"></a>00203 
<a name="l00204"></a>00204         <span class="comment">// Note: You can&#39;t select wether to include the intrinsic camera parameters (focal, radial distortion coefficients) in the optimization, or not.</span>
<a name="l00205"></a>00205         <span class="comment">// Seems that this SBA implementation only optimizes the camera pose.</span>
<a name="l00206"></a>00206 
<a name="l00207"></a>00207         <span class="comment">// Get the time of LM optimization only.</span>
<a name="l00208"></a>00208         QTime time;
<a name="l00209"></a>00209         time.start();
<a name="l00210"></a>00210         <span class="keyword">const</span> <span class="keywordtype">int</span> numPerformedIters = sys.doSBA(numIters, lambda, useCSparse, initTolCG, maxItersCG);
<a name="l00211"></a>00211         time_sSBA = time.elapsed();
<a name="l00212"></a>00212 
<a name="l00213"></a>00213         refinedPoints3D.clear();
<a name="l00214"></a>00214         refinedCameraPoses.clear();
<a name="l00215"></a>00215 
<a name="l00216"></a>00216         QList&lt; QHash&lt; int, QPointF&gt; &gt; refinedPointsProjections;
<a name="l00217"></a>00217         recoverReconstructionFromSysSBA(sys, refinedCameraPoses, refinedPoints3D, refinedPointsProjections);
<a name="l00218"></a>00218 
<a name="l00219"></a>00219 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l00220"></a>00220 <span class="preprocessor"></span>        <span class="keywordflow">if</span> (refinedPointsProjections != pointsProjections)
<a name="l00221"></a>00221                 {
<a name="l00222"></a>00222                 std::cout &lt;&lt; <span class="stringliteral">&quot;---------- Lambda ERROR x343432----------------------------------&quot;</span> &lt;&lt; std::endl;
<a name="l00223"></a>00223                 exit(0);
<a name="l00224"></a>00224                 }
<a name="l00225"></a>00225 <span class="preprocessor">        #endif</span>
<a name="l00226"></a>00226 <span class="preprocessor"></span>
<a name="l00227"></a>00227         <span class="keywordflow">return</span> numPerformedIters;
<a name="l00228"></a>00228         }
<a name="l00229"></a>00229 
<a name="l00230"></a>00230 
<a name="l00231"></a>00231 
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
